#include <cratedemo/Crate.hpp>
#include <cratedemo/MiniBall.hpp>
#include <iostream>
#include <cratedemo/initialCrateContentDemo1.hpp>
#include <cratedemo/CrateDemo.hpp>
#include <ros/ros.h>
#include <unistd.h>
#include <iostream>
#include <cratedemo/demoMsg.h>
#include <cratedemo/demoProgressMsg.h>
#include <cratedemo/TopViewMsg.h>

#include <datatypes/Color.hpp>

using namespace cratedemo;

class Demo : public CrateDemo
{
public:	
	int demoMode;
	
	int srcPos;
	int dstPos;
	
	std::string dstCrate;
	std::string srcCrate;
	
	ros::Publisher demo_progress;
	ros::Publisher top_request;
	
	ros::Subscriber sub;
	
	Demo(ros::NodeHandle& hNode,
		CrateContentMap& crateContentMap, int mode, int topview_sub):CrateDemo(hNode,
		"enableGripper" ,
		"stop",
		"moveTo" ,
		"checkTo" ,
		"deltaError" ,
		"getAllCrates",
		"getCrate",
		"crateEvent",
		"visionError",
		crateContentMap), demoMode(mode) {
		
		demoProgress = 0;
		
		if (topview_sub != 0)
			sub = hNode.subscribe("topview_info", 1000, &Demo::topviewCallback, this);
		
		demo_progress = hNode.advertise<cratedemo::demoProgressMsg>("demo_progress", 1000);

		top_request = hNode.advertise<cratedemo::TopViewMsg>("topview_request", 1000);		
		/*
		ros::Rate poll_rate(100);
		while(top_request.getNumSubscribers() == 0) {
    		poll_rate.sleep();
    	}*/
		ros::Rate loop_rate(10);		
	}

	~Demo() {}

	void onNewCrate(Crate& crate) {
		ROS_INFO("onNewCrate called %s, on location:\tx:\t%f\ty:\t%f",crate.getName().c_str(),crate.position.x,crate.position.y);			
		ROS_INFO("demoMOde %d", demoMode);
		
		if (!crate.isScanned) {
			TopViewMsg msg;
			
			CrateMsg cratemsg;
			cratemsg.name = crate.getName();
			cratemsg.x = crate.position.x;
			cratemsg.y = crate.position.y;
			
			cratemsg.angle = crate.angle;

			msg.crate = cratemsg;			
			
			top_request.publish(msg);
		}
		
		if(demoMode == 1){ // manual
			CrateMap::iterator it1 = crates.find(srcCrate);
			if(it1 == crates.end()) { return; }

			CrateMap::iterator it2 = crates.find(dstCrate);
			if(it2 == crates.end()) { return; }
		
			moveObject(*(it1->second), srcPos, *(it2->second), dstPos);
		} else if(demoMode != 0){
			demoSteps = 2;
		   //init voor non manual
		   CrateMap::iterator it1 = crates.find("GC4x4MB_1");
			if(it1 == crates.end()) { return; }

			CrateMap::iterator it2 = crates.find("GC4x4MB_2");
			if(it2 == crates.end()) { return; }
		   
		   //if list for all demo modes
		   if(demoMode == 2){ //direct copy
		   	demoSteps = 1;
		   	
		   	std::cout << "direct copy" << std::endl;
		   	std::cout << "direct copy: reset done" << std::endl;
				for(int y = 0; y < it1->second->grid_height && y < it2->second->grid_height; y++){
	   	     	for(int x = 0; x < it1->second->grid_width && x < it2->second->grid_width; x++){
	   	     		demoSteps++;
		    			moveObject(*(it1->second), x+y*it1->second->grid_width, *(it2->second),x+y*it2->second->grid_width);
	   	     	}
		    	}			
				std::cout << "direct copy done" << std::endl;
		   } else if(demoMode == 3){ //one spare hole
	      	int empty_x = 3;
	      	int empty_y = 3;
	      	
	      	demoSteps = 1;

	    		for(int y = 0; y < it1->second->grid_height && y < it2->second->grid_height; y++){
	   	     	for(int x = 0; x < it1->second->grid_width && x < it2->second->grid_width; x++){
	   	     		demoSteps+= 3;
		    			moveObject(*(it2->second), x+y*it2->second->grid_width, *(it2->second),empty_x+empty_y*it2->second->grid_width);
		 	   		moveObject(*(it1->second), x+y*it1->second->grid_width, *(it2->second), x+y*it2->second->grid_width);           
	      	      moveObject(*(it2->second), empty_x+empty_y*it2->second->grid_width, *(it1->second), x+y*it1->second->grid_width);
	   	     	}
		    	}
		   } else if(demoMode == 4){ //complete mirrored copy
		   	demoSteps = 1;
		   	
		   	for(int y = 0; y < it1->second->grid_height && y < it2->second->grid_height; y++){
					for(int x = 0; x < it1->second->grid_width && x < it2->second->grid_width; x++){
						if(1){ //plek is vol
							demoSteps++;
							moveObject(*(it1->second),  x+y*it1->second->grid_width, *(it2->second), (it2->second->grid_width-x-1)+(it2->second->grid_height-y-1)*it2->second->grid_width/*width*/);
						}
					}
				}
		   	
		   } else if(demoMode == 5){ // horizontal mirror
				demoSteps = 1;
				
		   	for(int y = 0; y < it1->second->grid_height && y < it2->second->grid_height; y++){
					for(int x = 0; x < it1->second->grid_width && x < it2->second->grid_width; x++){
						if(1){ //if voor plek vol/leeg
							demoSteps++;
							moveObject(*(it1->second),  x+y*it1->second->grid_width, *(it2->second), it2->second->grid_width - x - 1 +it2->second->grid_width*y);
						}
					}
				}
		   } else if(demoMode == 6){ // vertical mirror
		   	demoSteps = 1;
		   	for(int y = 0; y < it1->second->grid_height && y < it2->second->grid_height; y++){
					for(int x = 0; x < it1->second->grid_width && x < it2->second->grid_width; x++){
						if(1){ //if voor plek vol/leeg
							demoSteps++;
							moveObject(*(it1->second),  x+y*it1->second->grid_width, *(it2->second), x+it2->second->grid_width*(it2->second->grid_height-y-1));
						}
					}
				}
		   
		   } else if(demoMode == 7){ //backward diagonal mirror  
		   	demoSteps = 1; 	
		   	for(int y = 0; y < it1->second->grid_height && y < it2->second->grid_height; y++){
					for(int x = 0; x < it1->second->grid_width && x < it2->second->grid_width; x++){
						if(1){ //if voor plek vol/leeg
							demoSteps++;
							moveObject(*(it1->second),  x+y*it1->second->grid_width, *(it2->second), x*it2->second->grid_height+y);
						}
					}
				}
			} else if(demoMode == 8){ //forward diagonal mirror
				demoSteps = 1;
				for(int y = 0; y < it1->second->grid_height && y < it2->second->grid_height; y++){
					for(int x = 0; x < it1->second->grid_width && x < it2->second->grid_width; x++){
						if(1){ //if voor plek vol/leeg
							demoSteps++;
							moveObject(*(it1->second),  x+y*it1->second->grid_width, *(it2->second), it2->second->grid_height-y-1 + it2->second->grid_height*(it2->second->grid_width-x-1) /*width*/);
						}
					}
				}
			} else if(demoMode == 9 && it1->second->isScanned && it2->second->isScanned){ // Empty as many crates as possible
				int* crate2 = new int[it2->second->grid_width*it2->second->grid_height];
				
				//fill crate2 with currently known data
				for(int y = 0; y < it2->second->grid_height; y++){
					for(int x = 0; x < it2->second->grid_width; x++){
						if (static_cast<MiniBall*>(it1->second->data[x + y*it1->second->grid_width]) != NULL) {
							crate2[x + y*it2->second->grid_width] = 1;
						} else {
							crate2[x + y*it2->second->grid_width] = 0;
						}
					}					
				}
				
				for (int c1_y = 0; c1_y < it1->second->grid_height; c1_y++) {
					for (int c1_x = 0; c1_x < it1->second->grid_width; c1_x++) {
						if (static_cast<MiniBall*>(it1->second->data[c1_x + c1_y*it1->second->grid_width]) != NULL) {
							bool empty_found = false;
							for (int c2_y = 0; c2_y < it2->second->grid_height; c2_y++) {
								for (int c2_x = 0; c2_x < it2->second->grid_width; c2_x++) {
									if (crate2[c2_x + c2_y*it2->second->grid_width] == 0) {
										moveObject(*(it1->second), c1_x + c1_y*it1->second->grid_width, *(it2->second), c2_x + c2_y*it2->second->grid_width);
										
										crate2[c2_x + c2_y*it2->second->grid_width] = 1; 
										empty_found = true; 
										break;
									}
								}
								
								if (empty_found) break;
							}
						}
					}
				}
			} else if(demoMode == 10 && it1->second->isScanned && it2->second->isScanned){
				int* crate2 = new int[it2->second->grid_width*it2->second->grid_height];
				
				//fill crate2 with currently known data
				for (int y = 0; y < it2->second->grid_height; y++) {
					for (int x = 0; x < it2->second->grid_width; x++) {
						if (static_cast<MiniBall*>(it1->second->data[x + y*it1->second->grid_width]) != NULL) {
							crate2[x + y*it2->second->grid_width] = 1;
						} else {
							crate2[x + y*it2->second->grid_width] = 0;
						}
					}					
				}
				
				demoSteps = 1;
				datatypes::Color colour = datatypes::RED;
				int red = 0, green = 0, blue = 0;
				for (int y = 0; y < it1->second->grid_height; y++) {
					for (int x = 0; x < it1->second->grid_width; x++) {
						if (static_cast<MiniBall*>(it1->second->data[x + y*it1->second->grid_width]) != NULL) {
							if(static_cast<MiniBall*>(it1->second->data[x + y*it1->second->grid_width])->getColor() == datatypes::RED){
								red++;
							} else if(static_cast<MiniBall*>(it1->second->data[x + y*it1->second->grid_width])->getColor() == datatypes::BLUE){
								blue++;
							} else if(static_cast<MiniBall*>(it1->second->data[x + y*it1->second->grid_width])->getColor() == datatypes::GREEN){
								green++;
							}
						}		
					}
				}
				
				std::cout << "red: " << red << ", green: " << green << ", blue: " << blue << std::endl;

				if(green > red && green > blue){
					colour = datatypes::GREEN;
				} else if(blue > red && blue > green){
					colour = datatypes::BLUE;
				}
				
				for (int c1_y = 0; c1_y < it1->second->grid_height; c1_y++) {
					for (int c1_x = 0; c1_x < it1->second->grid_width; c1_x++) {
						if(static_cast<MiniBall*>(it1->second->data[c1_x + c1_y*it1->second->grid_width]) != NULL) {
							if(static_cast<MiniBall*>(it1->second->data[c1_x + c1_y*it1->second->grid_width])->getColor() == colour){
								bool empty_found = false;
								for (int c2_y = 0; c2_y < it2->second->grid_height; c2_y++) {
									for (int c2_x = 0; c2_x < it2->second->grid_width; c2_x++) {
										if (crate2[c2_x + c2_y*it2->second->grid_width] == 0) {
											moveObject(*(it1->second), c1_x + c1_y*it1->second->grid_width, *(it2->second), c2_x + c2_y*it2->second->grid_width);
										
											crate2[c2_x + c2_y*it2->second->grid_width] = 1; 
											empty_found = true; 
											break;
										}
									}
									
									if (empty_found) break;
								}
							}
						}
					}
				}
				
				demoMode = 0;
				
			} else if(demoMode == 11){
			} else if(demoMode == 12){
			}
		}  
			
		if (demoMode != 9 && demoMode != 10)
			demoMode = 0; //Demo initialization is finished for all demo's that do not need top vision input
		
		ROS_INFO("Demo done...");
	}

	void topviewCallback(const cratedemo::TopViewMsg::ConstPtr& msg){
		Crate* this_crate =
				crates.find(msg->name) != crates.end() ?
						crates.find(msg->name)->second : NULL;

		if(this_crate != NULL){
			for(int i1 = 0; i1 < msg->rows; i1++){
				for(int i2 = 0; i2 < msg->cols; i2++){
					delete this_crate->data[i2 + i1*msg->rows];
					this_crate->data[i2 + i1*msg->rows] = NULL;
					
					if (msg->data[i1 + i2*msg->rows] != 0) {
						if (msg->data[i1 + i2*msg->rows] == 1){
							std::cout << "new red" << std::endl;
							this_crate->data[i2 + i1*msg->rows] = new MiniBall(datatypes::RED);
						} else if (msg->data[i1 + i2*msg->rows] == 2){
							std::cout << "new green" << std::endl;
							this_crate->data[i2 + i1*msg->rows] = new MiniBall(datatypes::GREEN);
						} else if (msg->data[i1 + i2*msg->rows] == 3){
							std::cout << "new blue" << std::endl;
							this_crate->data[i2 + i1*msg->rows] = new MiniBall(datatypes::BLUE);
						}
					}
				}
			}
			std::cout << "New crate information used" << std::endl;
			this_crate->isScanned = true;
		}
		
		if (demoMode == 9 || demoMode == 10) {
			onNewCrate(*this_crate);
		}
	}
	
	void sendProgressInfo() {
		demoProgress++;
		
		//Send demo progress to huniplacer_3d	
		demoProgressMsg msg;
		msg.totalSteps = demoSteps;
		msg.currentStep = demoProgress;
						
		demo_progress.publish(msg);
	}

	void onCrateMove(Crate& crate)
	{
		ROS_INFO("onCrateMove called %s",crate.getName().c_str());
	}

	void onCrateRemoved(Crate& crate)
	{
		ROS_INFO("onCrateRemoved called %s",crate.getName().c_str());
	}

	void onDeltaError(int, const std::string&){
		// Do nothing
	}

	void onVisionError(int, const std::string&){
		// Do nothing
	}
	
	void setMode(int mode) {
		demoMode = mode;
	}
	
	void setSrcPos(int src){
		srcPos = src;
	}
	
	void setDstPos(int dst){
		dstPos = dst;
	}
	
	void setSrcCrate(std::string src){
		srcCrate = src;
	}
	
	void setDstCrate(std::string dst){
		dstCrate = dst;	
	}
};

Demo* current_demo;

int topview_sub;

using namespace std;

void onNewDemo(const demoMsg::ConstPtr& msg) {
	cout << "new demo mode: " << msg->demo << endl;
	
	delete current_demo;
	
	ros::NodeHandle nodeHandle;
	
	current_demo = new Demo(nodeHandle, initializeCrateContent1(msg->demo), msg->demo, topview_sub);
	current_demo->getAllCrates();
	
	current_demo->setSrcPos(msg->srcPos);
	current_demo->setDstPos(msg->dstPos);
	current_demo->setSrcCrate(msg->srcCrate);
	current_demo->setDstCrate(msg->dstCrate);
}

int main(int argc, char** argv) {
	if (argc != 3) {
		std::cout << "Usage for starting demo: /narg1: '1' for listening to crate_vision, '0' for not listening to crate_vision/narg2: path to crates.xml" << std::endl;
		exit(0);
	}
	topview_sub = *argv[1] - 48;

	Crate::crate_info = argv[2];	
	
	ros::init(argc, argv, "demoNode");
	ros::NodeHandle nodeHandle;
	ros::Subscriber sub2 = nodeHandle.subscribe("demo_info", 10, &onNewDemo);
	
	current_demo = new Demo(nodeHandle,initializeCrateContent1(0), 0, topview_sub);
	current_demo->getAllCrates();
	
	ros::spin();
	cout << "ros is not OK" << endl;
	return 0;
}
